#include <kuai/tool/xyz.h>

namespace kuai {

	RealNumber
		angle(
			const Coord3D& v1, const Coord3D& v2
		)
	{
		RealNumber r12 = sqrt(dot(v1, v1)*dot(v2, v2));
		if (fabs(r12) < EPSILON) {
			return 0;
		}
		else {
			RealNumber cos_result = dot(v1, v2)/r12;
			if (cos_result < -1) {
				return PI;
			}
			else if (cos_result > 1) {
				return 0;
			}
			else {
				return acos(cos_result);
			}
		}
	}

	Coord3D operator*(const Coord3D& v1, const TranslateMatrix& v2)
	{
		Coord3D result;
		result.x = v1.x * v2(0, 0) + v1.y * v2(1, 0) + v1.z * v2(2, 0) + v2(3, 0);
		result.y = v1.x * v2(0, 1) + v1.y * v2(1, 1) + v1.z * v2(2, 1) + v2(3, 1);
		result.z = v1.x * v2(0, 2) + v1.y * v2(1, 2) + v1.z * v2(2, 2) + v2(3, 2);
		RealNumber d = v1.x * v2(0, 3) + v1.y * v2(1, 3) + v1.z * v2(2, 3) + v2(3, 3);
		result /= d;
		return result;
	}

	Coord3D operator*(const TranslateMatrix& v2, const Coord3D& v1)
	{
		Coord3D result;
		result.x = v1.x * v2(0, 0) + v1.y * v2(0, 1) + v1.z * v2(0, 2) + v2(0, 3);
		result.y = v1.x * v2(1, 0) + v1.y * v2(1, 1) + v1.z * v2(1, 2) + v2(1, 3);
		result.z = v1.x * v2(2, 0) + v1.y * v2(2, 1) + v1.z * v2(2, 2) + v2(2, 3);
		RealNumber d = v1.x * v2(3, 0) + v1.y * v2(3, 1) + v1.z * v2(3, 2) + v2(3, 3);
		result /= d;
		return result;
	}

	TranslateMatrix TransMatrix(const Coord3D& v0)
	{
		TranslateMatrix result(0.0);
		result(0, 0) = 1;
		result(1, 1) = 1;
		result(2, 2) = 1;
		result(3, 3) = 1;
		result(3, 0) = v0.x;
		result(3, 1) = v0.y;
		result(3, 2) = v0.z;
		return result;
	}

	TranslateMatrix RotateMatrix(const Coord3D& vAxis, RealNumber angle)
	{
		RealNumber ll = 1/abs(vAxis);
		RealNumber a = vAxis.x * ll;
		RealNumber b = vAxis.y * ll;
		RealNumber c = vAxis.z * ll;

		RealNumber sina = sin(angle);
		RealNumber cosa = cos(angle);

		RealNumber result[] = {
			(1-a*a)*cosa + a*a,
			c*sina + a*b*(1 - cosa),
			-b*sina + a*c*(1 - cosa),
			0,

			a*b*(1-cosa) - c*sina,
			b*b+cosa*(1-b*b),
			b*c*(1-cosa) + a*sina,
			0,

			a*c*(1-cosa) + b*sina,
			b*c*(1-cosa) - a*sina,
			c*c + cosa*(1-c*c),
			0,

			0, 0, 0, 1
		};


		return TranslateMatrix(result);
	}

	TranslateMatrix RotateMatrix(const Coord3D& vAxis1, const Coord3D& vAxis2, RealNumber angle)
	{
		TranslateMatrix t1 = TransMatrix(-vAxis1);
		TranslateMatrix m = RotateMatrix(vAxis2 - vAxis1, angle);
		TranslateMatrix t2 = TransMatrix(vAxis1);
		TranslateMatrix result = t1;
		result *= m;
		result *= t2;
		return result;
	}
}
